#include "geometry.h"
#include <iostream>

Sphere::Sphere(const Vec3& ori, const float &rad)
{
	this->origin = ori;
	this->radius = rad;
	this->color.x = 0;
	this->color.y = 0;
	this->color.z = 0;
}
Sphere::Sphere(const Vec3& ori, const float &rad, const Color3& col)
{
	this->origin = ori;
	this->radius = rad;
	//printf("In: (%d, %d, %d)\n", col.x, col.y, col.z);
	this->color.x = col.x;
	this->color.y = col.y;
	this->color.z = col.z;
}
Sphere::Sphere(float x, float y, float z, const float &rad)
{
		Vec3 ori(x, y, z);
		this->origin = ori;
		this->radius = rad;
}
Color3 Sphere::getColor()
{
	return this->color;
}
Vec3 Sphere::getOrigin()
{
	return this->origin;
}
bool Sphere::intersect(const Ray& ray, float& dist, Color3& color, Vec3& intersect)
{
	// initialize color to black
	color.set(0.0, 0.0, 0.0);
	Vec3 start = this->origin - ray.origin;

	float a = ray.direction.dot( ray.direction );
	float b = ray.direction.dot( start ) * 2.0;
	float c = start.dot( start ) - ( this->radius * this->radius );
	float t = ( b * b ) - ( 4.0 * ( a * c ) );
	if ( t <= 0 ) // missed the sphere
		return false;

	float q = sqrtf( t );
	float sol_1 = (-b + q) / (2.0 * a);
	float sol_2 = (-b - q) / (2.0 * a);

	// pick the correct intersection:
	if ( sol_1 < 0 and sol_2 < 0 ) // if negative, they are behind the camera
		return false;
	//std::cout << "test";
	if ( sol_1 < 0 )
		dist = sol_2;
	else if ( sol_2 < 0 )
		dist = sol_1;
	else
	{
		// pick the smallest
		float test = sol_1 - sol_2;
		if ( test > 0 )
			dist = sol_2;
		else
			dist = sol_1;
	}
	color = this->getColor();

	//printf("%f\n", dist);
	intersect = (ray.origin - ray.origin - ray.origin) + ray.direction * dist;
	// printf("(%f, %f, %f)\n", intersect.x, intersect.y, intersect.z);

	return true;
	/*
	// initialize color to black
	color.set(0.0, 0.0, 0.0);
	// code adapated from Essential Mathmatics for Games and Interactive Applications
	// compute intermediate values
	Vec3 w = ray.origin - this->origin;
	float wsq = w.dot(w);
	float proj = w.dot(ray.direction);
	int radius2 = this->radius*this->radius;

	// if the sphere is behind the ray, no intersection
	if ( proj < 0.0f && wsq > radius2 )
	{
		return false;
	}
	else
	{
		float vsq = ray.direction.dot(ray.direction);
		dist = proj;
		// test length of difference vs. radius
		color = this->getColor();

		return ( vsq*wsq - proj*proj <= vsq*radius2 );
	}
	*/
}
